package com.J.Plane;

public class Quaternion {
	public float x;
	public float y;
	public float z;
	public float w;

	public static final Quaternion DEFAULT_ORIENTATION = new Quaternion(1.0f,
			0.0f, 0.0f, 0.0f);

	public Quaternion() {
		w = 1.0f;
		x = y = z = 0.0f;
	}

	public Quaternion(float w, float x, float y, float z) {
		this.w = w;
		this.x = x;
		this.y = y;
		this.z = z;
	}
	
	public void copyFrom(Quaternion rhs) {
		w = rhs.w;
		x = rhs.x;
		y = rhs.y;
		z = rhs.z;
	}
	
	public Quaternion clone() {
		return new Quaternion(w,x,y,z);
	}
	
	public void rotate(final Vector3 v, Vector3 rotated)
	{
		float quatW = this.w;
		float quatX = this.x;
		float quatY = this.y;
		float quatZ = this.z;
		
		float quatX2 = quatX * quatX;
		float quatY2 = quatY * quatY;
		float quatZ2 = quatZ * quatZ;
		float quatXY = quatX * quatY;
		float quatXZ = quatX * quatZ;
		float quatYZ = quatY * quatZ;
		float quatWX = quatW * quatX;
		float quatWY = quatW * quatY;
		float quatWZ = quatW * quatZ;
		
		float vX = v.x;
		float vY = v.y;
		float vZ = v.z;
		
		float rotatedX;
		float rotatedY;
		float rotatedZ;
		
		rotatedX = (1.0f - 2.0f * (quatY2 + quatZ2))*vX+(2.0f * (quatXY - quatWZ))*vY + (2.0f * (quatXZ + quatWY))*vZ;
		rotatedY = (2.0f * (quatXY + quatWZ))*vX+(1.0f - 2.0f * (quatX2 + quatZ2))*vY + (2.0f * (quatYZ - quatWX))*vZ;
		rotatedZ = (2.0f * (quatXZ - quatWY))*vX+(2.0f * (quatYZ + quatWX))*vY + (1.0f - 2.0f * (quatX2 + quatY2))*vZ;
		
		rotated.x = rotatedX;
		rotated.y = rotatedY;
		rotated.z = rotatedZ;
	}
	
	/** Convert Euler to quaternion */
	public void quaternionFromAngleAxis(float angleRadian, float x, float y, float z){

		
		float angle = (float) (angleRadian * 0.5);
		float norm = (float) Math.sqrt( x * x + y * y + z * z);
		float sinAngle = (float) Math.sin(angle);
		
		float normX = x/norm;
		float normY = y/norm;
		float normZ = z/norm;
		
		this.w = (float) Math.cos(angle);
		this.x = normX * sinAngle;
		this.y = normY * sinAngle;
		this.z = normZ * sinAngle;
		
	}
	
	

	/**
	 * Convert current quaternion value to 
	 * rotation matrix.
	 * 
	 * @param [out]mat	Output matrix
	 */
	public void toMatrix(Matrix44 mat) {
		
		float x2 = this.x * this.x;
		float y2 = this.y * this.y;
		float z2 = this.z * this.z;
		float xy = this.x * this.y;
		float xz = this.x * this.z;
		float yz = this.y * this.z;
		float wx = this.w * this.x;
		float wy = this.w * this.y;
		float wz = this.w * this.z;
		
		mat.n00 = 1.0f - 2.0f * (y2 + z2);
		mat.n01 = 2.0f * (xy - wz);
		mat.n02 = 2.0f * (xz + wy);
		mat.n03 = 0.0f;
		
		mat.n10 = 2.0f * (xy + wz);
		mat.n11 = 1.0f - 2.0f * (x2 + z2);
		mat.n12 = 2.0f * (yz - wx);
		mat.n13 = 0.0f;
		
		mat.n20 = 2.0f * (xz - wy);
		mat.n21 = 2.0f * (yz + wx);
		mat.n22 = 1.0f - 2.0f * (x2 + y2);
		mat.n23 = 0.0f;
		
		mat.n30 = 0.0f;
		mat.n31 = 0.0f;
		mat.n32 = 0.0f;
		mat.n33 = 1.0f;
		
		// Temporary
		//mat.identity();
	}
	
	public static void NormalizingQuaternion(Quaternion q){
		float normQ = (float) Math.sqrt(q.w * q.w +q.x * q.x +q.y * q.y +q.z * q.z );
		
		float normQW = q.w / normQ;
		float normQX = q.x / normQ;
		float normQY = q.y / normQ;
		float normQZ = q.z / normQ;
		
		q.w = normQW;
		q.x = normQX;
		q.y = normQY;
		q.z = normQZ;
	}
	
	public static Quaternion multiply(Quaternion q1, Quaternion q2, Quaternion qr) {

		/** Brian - Quaternion Multiply. Note that it is non-commutative */
		/** Normailze quaternion first to avoid the commulative bounded error*/
		/**
		float normQ1 = (float) Math.sqrt(q1.w * q1.w +q1.x * q1.x +q1.y * q1.y +q1.z * q1.z );
		float normQ2 = (float) Math.sqrt(q2.w * q2.w +q2.x * q2.x +q2.y * q2.y +q2.z * q2.z );
		
		float normQ1W = q1.w / normQ1;
		float normQ1X = q1.x / normQ1;
		float normQ1Y = q1.y / normQ1;
		float normQ1Z = q1.z / normQ1;
		
		float normQ2W = q2.w / normQ2;
		float normQ2X = q2.x / normQ2;
		float normQ2Y = q2.y / normQ2;
		float normQ2Z = q2.z / normQ2;
		*/

		/**
		qr.w = normQ1W*normQ2X + normQ1X*normQ2W + normQ1Y*normQ2Z - normQ1Z*normQ2Y;
        qr.x = normQ1W*normQ2Y + normQ1Y*normQ2W + normQ1Z*normQ2X - normQ1X*normQ2Z;
        qr.y = normQ1W*normQ2Z + normQ1Z*normQ2W + normQ1X*normQ2Y - normQ1Y*normQ2X;
        qr.z = normQ1W*normQ2W - normQ1X*normQ2X - normQ1Y*normQ2Y - normQ1Z*normQ2Z;
        */
		NormalizingQuaternion(q1);
		NormalizingQuaternion(q2);
		
		float q1W = q1.w;
		float q1X = q1.x;
		float q1Y = q1.y;
		float q1Z = q1.z;
		
		float q2W = q2.w;
		float q2X = q2.x;
		float q2Y = q2.y;
		float q2Z = q2.z;
		
		qr.x = q1W*q2X + q1X*q2W + q1Y*q2Z - q1Z*q2Y;
        qr.y = q1W*q2Y + q1Y*q2W + q1Z*q2X - q1X*q2Z;
        qr.z = q1W*q2Z + q1Z*q2W + q1X*q2Y - q1Y*q2X;
        qr.w = q1W*q2W - q1X*q2X - q1Y*q2Y - q1Z*q2Z;
        
		return qr;
	}
	
	
}
